// Include files.
#include "Constants.hpp"
#include "TestRobot.hpp"

TestRobot::TestRobot() :
	m_drive(),
	m_periodic(),
	m_joystickLeft(PORT_DRIVER_JOYSTICK_LEFT),
	m_joystickRight(PORT_DRIVER_JOYSTICK_RIGHT),
	m_joystick(PORT_XBOX_DRIVER_JOYSTICK),
	m_ps3Joystick(PORT_PS3_DRIVER_JOYSTICK),
	m_digitalInputDriveInputUp(m_joystickRight, 4),
	m_digitalInputDriveInputDown(m_joystickRight, 5),
	m_digitalInputDriveModeTwoJoysticksUp(m_joystickLeft, 4),
	m_digitalInputDriveModeTwoJoysticksDown(m_joystickLeft, 5),
	m_digitalInputDriveModeXboxUp(m_joystick, XboxJoystickInput::kBack),
	m_digitalInputDriveModeXboxDown(m_joystick, XboxJoystickInput::kStart),
	m_digitalInputDriveModePs3Up(m_ps3Joystick, PS3JoystickInput::kSquare),
	m_digitalInputDriveModePs3Down(m_ps3Joystick, PS3JoystickInput::kCircle),
	m_outputDriveMode(*SmartDashboard::GetInstance(), "Drive Mode"),
	//m_controllerAutonomous(*this),
	m_controllerTestDriveDistance(m_drive, *(new GenericHidDigitalInput(m_joystickRight, 6))),
	m_controllerTeleop(),
	m_controllerDriveJoystickSelector
	(
		m_digitalInputDriveInputUp, 
		m_digitalInputDriveInputDown, 
		m_outputDriveMode
	),
	m_controllerDriveSelectorTwoJoysticks
	(
		m_digitalInputDriveModeTwoJoysticksUp,
		m_digitalInputDriveModeTwoJoysticksDown,
		m_outputDriveMode
	),
	m_controllerDriveSelectorXbox
	(
		m_digitalInputDriveModeXboxUp,
		m_digitalInputDriveModeXboxDown,
		m_outputDriveMode
	),
	m_controllerDriveSelectorPs3
	(
		m_digitalInputDriveModePs3Up,
		m_digitalInputDriveModePs3Down,
		m_outputDriveMode
	),
	m_controllerDriveTankPassiveMecanum(m_drive, m_joystickLeft, m_joystickRight),
	m_controllerDriveTankActiveMecanum(m_drive, m_joystickLeft, m_joystickRight),
	m_controllerDriveArcadeNormalMecanum(m_drive, m_joystickLeft, m_joystickRight),
	m_controllerDriveArcadeModernMecanum(m_drive, m_joystickLeft, m_joystickRight),
	m_xboxArcadeDriveModernStyleWithMecanumController(m_drive, m_joystick),
	m_xboxTankDriveWithPassiveMecanumController(m_drive, m_joystick),
	m_xboxArcadeDriveNormalStyleWithMecanumController(m_drive, m_joystick),
	m_ps3TankDriveWithPassiveMecanumController(m_drive, m_ps3Joystick),
	m_ps3ArcadeDriveModernStyleWithMecanumController(m_drive, m_ps3Joystick),
	m_ps3ArcadeDriveNormalStyleWithMecanumController(m_drive, m_ps3Joystick),
	m_ps3TiltDriveStyleWithMecanumController(m_drive, m_ps3Joystick)
{
	printf("Robot: Constructed\n");
}

void TestRobot::Initialize()
{
	printf("Robot: Initalizing\n");
	
	// Initialize SmartDashboard.
	SmartDashboard::GetInstance()->init();
	
	// Add subsystems.
	AddSubsystem(m_drive);
	//AddSubsystem(m_periodic);
	
	printf("Robot: Drive Timeout is %f.\n", m_drive.GetExpiration());
	
	// Add controllers.
	AddController(m_controllerAutonomous);
	AddController(m_controllerTeleop);
	SetAutonomousController(m_controllerAutonomous);
	SetTeleopController(m_controllerTeleop);
	
	// Configure controllers
	// Master input type selector
	m_controllerDriveJoystickSelector.AddItem("Two Joysticks", &m_controllerDriveSelectorTwoJoysticks);
	m_controllerDriveJoystickSelector.AddItem("Xbox", &m_controllerDriveSelectorXbox);
	m_controllerDriveJoystickSelector.AddItem("Ps3", &m_controllerDriveSelectorPs3);
	
	// Two joystick drive mode selector
	m_controllerDriveSelectorTwoJoysticks.AddItem("Tank Drive Passive Mecanum", &m_controllerDriveTankPassiveMecanum);
	m_controllerDriveSelectorTwoJoysticks.AddItem("Tank Drive Active Mecanum", &m_controllerDriveTankActiveMecanum);
	m_controllerDriveSelectorTwoJoysticks.AddItem("Arcade Normal", &m_controllerDriveArcadeNormalMecanum);
	m_controllerDriveSelectorTwoJoysticks.AddItem("Arcade Modern", &m_controllerDriveArcadeModernMecanum);
	m_controllerDriveSelectorTwoJoysticks.AddItem("Drive Distance Test", &m_controllerTestDriveDistance);

	// Xbox joystick drive mode selector
	m_controllerDriveSelectorXbox.AddItem("Arcade Modern", &m_xboxArcadeDriveModernStyleWithMecanumController);
	m_controllerDriveSelectorXbox.AddItem("Tank Drive Passive Mecanum", &m_xboxTankDriveWithPassiveMecanumController);
	m_controllerDriveSelectorXbox.AddItem("Arcade Normal", &m_xboxArcadeDriveNormalStyleWithMecanumController);

	// Ps3 joystick drive mode selector
	m_controllerDriveSelectorPs3.AddItem("Tank Drive Passive Mecanum", &m_ps3TankDriveWithPassiveMecanumController);
	m_controllerDriveSelectorPs3.AddItem("Arcade Modern", &m_ps3ArcadeDriveModernStyleWithMecanumController);
	m_controllerDriveSelectorPs3.AddItem("Arcade Normal", &m_ps3ArcadeDriveNormalStyleWithMecanumController);
	m_controllerDriveSelectorPs3.AddItem("Tilt Style", &m_ps3TiltDriveStyleWithMecanumController);

	printf("Haven't crashed yet.");
	
	m_controllerTeleop.AddController(m_controllerDriveJoystickSelector);
	
	//set up drive controller
	SetupDriveController<TankDriveWithPassiveMecanumController>(m_controllerDriveTankPassiveMecanum);
	SetupDriveController<TankDriveWithActiveMecanumController>(m_controllerDriveTankActiveMecanum);
	SetupDriveController<ArcadeDriveNormalStyleWithMecanumController>(m_controllerDriveArcadeNormalMecanum);
	SetupDriveController<ArcadeDriveModernStyleWithMecanumController>(m_controllerDriveArcadeModernMecanum);
	SetupDriveController<XboxArcadeDriveModernStyleWithMecanumController>(m_xboxArcadeDriveModernStyleWithMecanumController);
	SetupDriveController<XboxTankDriveWithPassiveMecanumController>(m_xboxTankDriveWithPassiveMecanumController);
	SetupDriveController<XboxArcadeDriveNormalStyleWithMecanumController>(m_xboxArcadeDriveNormalStyleWithMecanumController);
	SetupDriveController<PS3TankDriveWithPassiveMecanumController>(m_ps3TankDriveWithPassiveMecanumController);
	SetupDriveController<PS3ArcadeDriveModernStyleWithMecanumController>(m_ps3ArcadeDriveModernStyleWithMecanumController);
	SetupDriveController<PS3ArcadeDriveNormalStyleWithMecanumController>(m_ps3ArcadeDriveNormalStyleWithMecanumController);
	SetupDriveController<PS3TiltDriveStyleWithMecanumValueFetcher>(m_ps3TiltDriveStyleWithMecanumController);
	
	printf("Robot: Initialized\n");
}
